import numpy as np
class PointProcessor:
    def __init__(self,yaml=None,calibration=None) -> None:
        self.calibration = calibration

    def get_edge_uvs(self, p1, p2, num_points, start_ratio=0.0, end_ratio=1.0, include_end=True):
        """
        获取两点之间的均匀采样点
        参数:
            p1: 起点 (u1, v1)
            p2: 终点 (u2, v2)
            num_points: 要生成的点的数量
            start_ratio: 起始位置比例 (0.0-1.0)
            end_ratio: 结束位置比例 (0.0-1.0)
            include_end: 是否包含结束点
        """
        if num_points <= 0:
            return []
        
        u1, v1 = p1
        u2, v2 = p2
        
        # 计算实际要生成的点的数量
        actual_num = num_points if include_end else num_points + 1
        
        # 生成线性插值参数
        t = np.linspace(start_ratio, end_ratio, actual_num)
        
        # 计算插值点
        points = []
        for ratio in t:
            u = u1 + ratio * (u2 - u1)
            v = v1 + ratio * (v2 - v1)
            points.append((u, v))
        
        # 如果不包含结束点，则移除最后一个点
        if not include_end and len(points) > 0:
            points = points[:-1]
        
        return points

        
    def get_dict_edges(self, pose, num_points):
        # 根据pose获取均匀取样点的像素坐标
        res = {}
        if pose is None or len(pose) == 0:
            print("未检测到角点，跳过。")
            return res
        
        # 获取所有可能的角点
        left_top = pose.get("left_top")
        left_bottom = pose.get("left_bottom")
        right_top = pose.get("right_top")
        right_bottom = pose.get("right_bottom")
        left_0 = pose.get("left_0")
        top_0 = pose.get("top_0")
        bottom_0 = pose.get("bottom_0")
        
        def get_segmented_edge_uvs(p1, p2, num_points):
            """
            在两点之间均匀生成指定数量的点
            
            参数:
                p1: 起点 (u1, v1)
                p2: 终点 (u2, v2)
                num_points: 要生成的点的总数(包含端点)
            
            返回:
                沿线段均匀分布的点列表
            """
            points = []
            if num_points <= 0:
                return points
            
            u1, v1 = p1
            u2, v2 = p2
            
            # 均匀分布参数
            for i in range(num_points):
                ratio = i / (num_points - 1) if num_points > 1 else 0
                u = u1 + ratio * (u2 - u1)
                v = v1 + ratio * (v2 - v1)
                points.append((u, v))
            
            return points

        
        # 获取左边缘 - 优先使用left_bottom和left_top，如果没有则尝试left_0和left_top
        if left_top is not None:
            if left_bottom is not None:
                res["left_edge"] = get_segmented_edge_uvs(left_bottom, left_top, num_points)
            elif left_0 is not None:
                res["left_edge"] = get_segmented_edge_uvs(left_0, left_top, num_points)
            else:
                res["left_edge"] = []
        else:
            res["left_edge"] = []
        
        # 获取右边缘 - 优先使用right_bottom和right_top
        if right_top is not None and right_bottom is not None:
            res["right_edge"] = get_segmented_edge_uvs(right_bottom, right_top, num_points)
        else:
            res["right_edge"] = []
        
        # 获取上边缘 - 优先使用left_top和right_top，如果没有则尝试left_top和top_0
        if left_top is not None:
            if right_top is not None:
                res["top_edge"] = get_segmented_edge_uvs(left_top, right_top, num_points)
            elif top_0 is not None:
                res["top_edge"] = get_segmented_edge_uvs(left_top, top_0, num_points)
            else:
                res["top_edge"] = []
        else:
            res["top_edge"] = []
        
        # 获取下边缘 - 优先使用left_bottom和right_bottom，如果没有则尝试bottom_0和left_bottom
        if left_bottom is not None:
            if right_bottom is not None:
                res["bottom_edge"] = get_segmented_edge_uvs(left_bottom, right_bottom, num_points)
            elif bottom_0 is not None:
                res["bottom_edge"] = get_segmented_edge_uvs(bottom_0, left_bottom, num_points)
            else:
                res["bottom_edge"] = []
        else:
            res["bottom_edge"] = []
        
        return res

    def get_dict_depth(self, dict_points):
        """
        对于输入的dict，返回dict里面所有点的深度值。
        输入:
            dict_points: 形如{"left_top": (u, v), "left_edge": [(u1,v1), (u2,v2), ...], ...}
            depth_img: 深度图
        返回:
            一个dict，key与输入一致，value为对应点的深度（单点为float，点集为list[float]）
        """
        # print(aligned_depth.shape)
        result = {}
        for key, value in dict_points.items():
            if value is None:
                result[key] = None
            elif isinstance(value, (list, tuple)) and len(value) > 0 and isinstance(value[0], (list, tuple)):
                # 是点集
                depths = []
                for pt in value:
                    if pt is None or len(pt) != 2:
                        depths.append(None)
                        continue
                    u, v = int(pt[0]), int(pt[1])
                    # 直接用get_depth读取，不做change
                    depth = self.calibration.get_depth(u, v, edge_name=key)
                    depths.append(depth)
                result[key] = depths
            elif isinstance(value, (list, tuple)) and len(value) == 2:
                # 单个点
                u, v = int(value[0]), int(value[1])
                depth = self.calibration.get_depth(u, v)
                result[key] = depth
            else:
                result[key] = None
        return result
    
    def get_dict_3dpoints(self,edges,edges_depth):
        edges_3d = {}
        for edge_name, points in edges.items():
            depths = edges_depth.get(edge_name, [])
            edge_3d_points = []
            for idx, pt in enumerate(points):
                if pt is not None and len(pt) == 2:
                    u, v = int(pt[0]), int(pt[1])
                    depth = None
                    if idx < len(depths):
                        depth = depths[idx]
                    if depth is not None and depth > 0:
                        pt_3d = self.calibration.pixel_to_3d(u, v, depth)
                        edge_3d_points.append(pt_3d)
                    else:
                        edge_3d_points.append(None)
                else:
                    edge_3d_points.append(None)
            edges_3d[edge_name] = edge_3d_points
        return edges_3d
    
######## # 以下为弃用代码 #########
        # def fitted_plane(self, edges):
        #     """
        #     根据edges中的["left_edge"] 和 ["bottom_edge"]拟合一个平面，
        #     并将所有点投影到该平面上，返回新的字典。
        #     输入:
        #         edges: dict, 形如{"left_edge": [pt1, pt2, ...], "bottom_edge": [pt1, ...], ...}
        #     返回:
        #         dict, 结构与输入一致，所有点都被映射到拟合平面上
        #     """
        #     import numpy as np

        #     # 收集拟合平面的点
        #     pts = []
        #     for edge_name in ["left_edge", "bottom_edge"]:
        #         if edge_name in edges:
        #             for pt in edges[edge_name]:
        #                 if pt is not None and hasattr(pt, '__len__') and len(pt) == 3:
        #                     pts.append(pt)
        #     if len(pts) < 3:
        #         print("拟合平面点数不足，无法拟合平面")
        #         return None

        #     pts = np.array(pts)
        #     # 拟合平面: ax+by+cz+d=0
        #     centroid = np.mean(pts, axis=0)
        #     uu, dd, vv = np.linalg.svd(pts - centroid)
        #     normal = vv[2, :]
        #     a, b, c = normal
        #     d = -np.dot(normal, centroid)

        #     def project_point_to_plane(pt, normal, d):
        #         pt = np.array(pt)
        #         # 平面法向量归一化
        #         n = normal / np.linalg.norm(normal)
        #         # 点到平面距离
        #         dist = (np.dot(n, pt) + d)
        #         # 投影点
        #         proj = pt - dist * n
        #         return proj

        #     # 映射所有点
        #     mapped_edges = {}
        #     for edge_name, points in edges.items():
        #         mapped_points = []
        #         for pt in points:
        #             if pt is not None and hasattr(pt, '__len__') and len(pt) == 3:
        #                 mapped_pt = project_point_to_plane(pt, normal, d)
        #                 mapped_points.append(mapped_pt.tolist())
        #             else:
        #                 mapped_points.append(None)
        #         mapped_edges[edge_name] = mapped_points

        #     return mapped_edges
        


        # def visual_point_dict(self, img, dict):
        #     # 绘制角点
        #     for name, pt in dict.items():
        #         if pt is not None and isinstance(pt, (list, tuple)) and len(pt) == 2 and isinstance(pt[0], (int, float)):
        #             cv2.circle(img, (int(pt[0]), int(pt[1])), 6, (0, 0, 255), -1)
        #             cv2.putText(img, name, (int(pt[0]) + 5, int(pt[1]) - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)

        #     # 自动分配颜色
        #     import colorsys
        #     edge_names = [k for k, v in dict.items() if isinstance(v, list) or isinstance(v, tuple) and len(v) > 2]
        #     n = len(edge_names)
        #     color_map = {}
        #     for i, edge_name in enumerate(edge_names):
        #         # 生成HSV颜色，转换为BGR
        #         h = i / max(n, 1)
        #         s = 1.0
        #         v = 1.0
        #         r, g, b = colorsys.hsv_to_rgb(h, s, v)
        #         color = (int(b * 255), int(g * 255), int(r * 255))  # OpenCV用BGR
        #         color_map[edge_name] = color

        #     # 绘制边采样点
        #     for edge_name, points in dict.items():
        #         if not (isinstance(points, list) or (isinstance(points, tuple) and len(points) > 2)):
        #             continue
        #         color = color_map.get(edge_name, (0, 255, 0))
        #         for idx, pt in enumerate(points):
        #             if not (isinstance(pt, (list, tuple)) and len(pt) == 2):
        #                 continue
        #             u, v = pt
        #             cv2.circle(img, (int(u), int(v)), 4, color, -1)
        #             # 只在第一个点标注边名
        #             if idx == 0:
        #                 cv2.putText(img, edge_name, (int(u) + 5, int(v) - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1)

        #     cv2.imshow("边采样结果", img)
        #     cv2.waitKey(0)
        #     cv2.destroyAllWindows()

        # def get_3d_pose(self, pose, depth_img):
        #     pose_3d = {}
        #     if not pose:
        #         print("没有计算出poly_pose")
        #         return pose_3d
        #     aligned_depth = self.calibration.align_depth_to_rgb(depth_img)
        #     self.calibration.group2_images['depth_aligned'] = aligned_depth
        #     for name, pt in pose.items():
        #         u, v = int(pt[0]), int(pt[1])
        #         depth = self.calibration.get_robust_depth_at_rgb_coord(u, v, group=2)
        #         # print(depth)
        #         pt_3d = self.calibration.pixel_to_3d(u, v, depth)
        #         pose_3d[name] = pt_3d
        #     return pose_3d



        # def get_edge_3d_points(self, pose, depth_img, num_samples=50):
        #     if not pose:
        #         print("没有计算出poly_pose")
        #         return {}

        #     aligned_depth = self.calibration.align_depth_to_rgb(depth_img)
        #     self.calibration.group2_images['depth_aligned'] = aligned_depth

        #     # 检查关键点是否存在
        #     left_top = np.array(pose["left_top"]) if "left_top" in pose else None
        #     left_bottom = np.array(pose["left_bottom"]) if "left_bottom" in pose else None
        #     right_bottom = np.array(pose["right_bottom"]) if "right_bottom" in pose else None

        #     result = {}

        #     # 采样left_edge
        #     left_edge_points = []
        #     points_depth=[]
        #     sample_points=[]
        #     if left_top is not None and left_bottom is not None:
        #         for t in np.linspace(0, 1, num_samples):
        #             pt = (1 - t) * left_top + t * left_bottom
        #             u, v = int(pt[0]), int(pt[1])
        #             depth = self.calibration.get_robust_depth_at_rgb_coord(u, v, group=2)
        #             points_depth.append(depth)
        #             sample_points.append((u, v))
        #     else:
        #         print("left_top 或 left_bottom 不存在，无法采样left_edge。")
        #     # points_depth=[] 应该大致为等差数列，其中存在一些明显偏离的点，将其修改为合理的值
        #     # points_depth=self.fitted_depth(points_depth)
            
        #     # 根据sample_points和points_depth生成left_edge_points
        #     for (u, v), depth in zip(sample_points, points_depth):
        #         pt_3d = self.calibration.pixel_to_3d(u, v, depth)
        #         if pt_3d is not None:
        #             left_edge_points.append(pt_3d)
        #     result["left_edge"] = left_edge_points
        #     result["fitted_left_edge"]=self.fitted_points(left_edge_points)

        #     # 采样bottom_edge
        #     bottom_edge_points = []
        #     if left_bottom is not None and right_bottom is not None:
        #         for t in np.linspace(0, 1, num_samples):
        #             pt = (1 - t) * left_bottom + t * right_bottom
        #             u, v = int(pt[0]), int(pt[1])
        #             depth = self.calibration.get_robust_depth_at_rgb_coord(u, v, group=2)
        #             print(depth)
        #             pt_3d = self.calibration.pixel_to_3d(u, v, depth)
        #             bottom_edge_points.append(pt_3d)
        #     else:
        #         print("left_bottom 或 right_bottom 不存在，无法采样bottom_edge。")
        #     result["bottom_edge"] = bottom_edge_points
        #     result["fitted_bottom_edge"]=self.fitted_points(bottom_edge_points)
        #     return result

        # def fitted_depth(self, depths, threshold=0.15, window_size=5):
        #     """
        #     对一维深度值列表进行拟合修正，处理异常值
            
        #     参数:
        #         depths: 原始深度值列表 [d1, d2, ...]
        #         threshold: 异常值检测阈值(相对拟合线的偏差比例)
        #         window_size: 用于局部平滑的窗口大小(奇数)
                
        #     返回:
        #         修正后的深度值列表
        #     """
        #     if not depths or len(depths) < 3:
        #         return depths
            
        #     depths = np.array(depths)
        #     indices = np.arange(len(depths))
            
        #     # 1. 初步线性拟合
        #     try:
        #         coeffs = np.polyfit(indices, depths, 1)
        #         fit_fn = np.poly1d(coeffs)
        #         fitted_values = fit_fn(indices)
        #     except:
        #         # 如果拟合失败(如所有点相同)，使用中值滤波
        #         return median_filter(depths, size=window_size).tolist()
            
        #     # 2. 计算相对偏差
        #     relative_errors = np.abs(depths - fitted_values) / (fitted_values + 1e-6)
            
        #     # 3. 修正异常点
        #     corrected_depths = depths.copy()
        #     outlier_mask = relative_errors > threshold
            
        #     if np.any(outlier_mask):
        #         # 使用有效点的线性插值替换异常点
        #         valid_indices = indices[~outlier_mask]
        #         valid_depths = depths[~outlier_mask]
                
        #         if len(valid_indices) > 1:
        #             interp_fn = np.interp(indices, valid_indices, valid_depths)
        #             corrected_depths[outlier_mask] = interp_fn[outlier_mask]
        #         else:
        #             # 如果没有足够有效点，使用拟合值
        #             corrected_depths[outlier_mask] = fitted_values[outlier_mask]
            
        #     # 4. 局部平滑处理
        #     if window_size > 1:
        #         corrected_depths = uniform_filter1d(
        #             corrected_depths, 
        #             size=window_size, 
        #             mode='nearest'
        #         )
            
        #     return corrected_depths.tolist()

        # edges1,edges2 :result

        
